
function res = trebufunV2_energyvalid(theta,phi,t,R,L1, L2,M1,M2,g)
phi = (phi/180)*pi;
theta = (theta/180) *pi;
trebvars = [theta;phi;0;0] ;
options = odeset('Events', @events);

InitialPE = -M1*g*R*sin(theta)+M2*g*L1*sin(theta)+M2*g*L2*sin(phi)


    function [value,isterminal,direction] = events(t,trebvars)
       if trebvars(1) - trebvars(2) == 0;
           value = 0;
       else
           value = 1;
       end
       
        isterminal = 1;
        direction = 1;
    end
[T,K] = ode45(@(t,trebvars)acceleration(trebvars,L1,L2,M1,M2,R,g),[0,t],trebvars,options);

theta = K(:,1);
phi = K(:,2);
thetadot = K(:,3);
phidot = K(:,4);
 for i = 1:length(theta)
 TotalE(i) = InitialPE
 KE(i) = 1/2*M1*(R^2)*(thetadot(i)^2)+1/2*M2*((L1^2)*(thetadot(i)^2)+(L2^2)*(phidot(i)^2) + 2*L1*L2*thetadot(i)*phidot(i)*cos(theta(i)-phi(i)));
 PE(i) = -M1*g*R*sin(theta(i))+M2*g*L1*sin(theta(i))+M2*g*L2*sin(phi(i));
Energy(i) = KE(i)+PE(i);
 end
 KE(1)
 PE(1)
 clf
thetadot(1)
phidot(1)
Energy(1)
 plot(T,Energy,'b')
 hold on
 plot(T,TotalE,'r')
 plot(T,KE,'k')
  plot(T,PE,'m')
  
end

function acc=acceleration(trebvars,L1,L2,M1,M2,R,g)
 theta = trebvars(1);
phi = trebvars(2);
thetadot = trebvars(3);
phidot = trebvars(4);


leftover = [M1*g*R*cos(theta)-M2*g*L1*cos(theta)-M2*L1*L2*(phidot^2)*sin(theta-phi);
           -M2*g*L2*cos(phi)+ M2*L1*L2*(thetadot^2)*sin(theta-phi)]; 
        %right side of matrix equation, top is theta
        
dotstuff = [(M1*R^2+M2*L1^2),M2*L1*L2*cos(theta-phi);
             L1*L2*M2*cos(theta-phi),M2*L2^2 ] ;             
         %left side of matrix equation, top is theta
      
doubledot =  inv(dotstuff)*leftover; 
%calulates theta and phi umblat, in that order
    

acc = [thetadot;phidot;doubledot(1);doubledot(2)];
end



